# -*- coding: utf-8 -*-
"""
Created on Sat Dec 18 21:04:25 2021

@author: wulong
"""
import numpy as np
from casadi import *
from Basic_paras import *

# In[1] Fast MPC model
# States
def ode_ies_f(xf, xs, xm, us, um, uf, uz, ud):
    dxdt = SX.zeros(Nx_f)
    
    SOC = xs[0]
    mstc = xs[1]
    mt_stc = xs[2]
    mt_sth = xs[3]
    tbr = xs[4]
    
    pH2O = xm[0]
    pH2 = xm[1]
    Pmtf = xm[2]
    tabf = xm[3]
    tabw = xm[4]
    tabt = xm[5]
    tcwm = xm[6]
    tewm = xm[7]
    tre = xm[8]
    
    If = xf[0]
    Gh2 = xf[1]
    pO2 = xf[2]
    tc = xf[3]
    tcs = xf[4]
    te = xf[5]
    tes = xf[6]
    vcap = xf[7]
    Iba = xf[8]
    
    Gab = us[0]
    Gec = us[1]
    Gstu = us[2]
    
    Gfm = um[0]
    
    Gff = uf[0]
    Nec = uf[1]
    
    zfc = uz[0]
    zma = uz[1]
    zec = uz[2]
    zst = uz[3]
    
    ta = ud[0]
    ins = ud[1]
    Pd = ud[2]
    Qother = ud[3]
    
    # PV
    I_pv_max = I_max_0*ins/Ins_pv_0*(1 + c_pv1*(ta - T_pv_0))
    V_pv_max = V_max_0*log(exp(1) + c_pv2*(ins - Ins_pv_0))*(1 - c_pv3*(ta - T_pv_0))
    Ppv = npp*nsp*I_pv_max*V_pv_max/1000
    
    # FC
    eta_a = afc + bfc*log(If + eps)
    eta_c = -R0fc*T0fc/(2*F0)*log(1-If/IL + eps)
    eta_o = If*rfc
    V0 = N0fc*(E0fc + R0fc*T0fc/(2*F0)*log(pH2*sqrt(pO2)/(pH2O + eps) + eps))
    Vfc = V0 - eta_a - eta_c - eta_o
    Ifc = fu_d/(2*Kr)*Gh2
    Pfc = zfc*(Vfc*Ifc/1000)
    Gff_mol = Gff*Mch4
    Go2 = 1/r_HO*Gh2
    
    # PIP in the rerurn side
    Gall = Gab + Gec + Gstu
    
    # MA
    Pmt = zma*(Pmt0 + Pmtf)
    
    # EC
    pc = exp(21.3-2025.5/(248.94+tc))/1e6
    pe = exp(21.3-2025.5/(248.94+te))/1e6
    hcro = -137.26+1.23463*(273.15+tc) - hspc
    hero = 338.02893+0.24532*(273.15+te) + hsph
    yb = pc/pe # Compressor
    etavl = 0.98-0.085*(yb**(1/kr)-1)
    etacp = 0.9085*exp(-0.06443*yb)-7.605*exp(-3.155*yb)
    Gr = etavl*Nec*roeg*Vcp
    wi = kr/(1e3*(kr-1))*pe*1e6/roeg*(yb**((kr-1)/kr)-1)
    hcri = hero + wi
    heri = hcro # Expansion valve
    Pcp = zec*(Gr*wi/etacp)
    
    # BR
    Hpmp = Spmp*Gall**2/(rhow*ge)
    eta_pmp = b0p*Gall**2 + b1p*Gall + b2p
    Ppmp = Gall*ge*Hpmp/(1e3*eta_pmp)
    
    # BA
    iba = Iba/npb
    Vba = nsb*(Em - vcap - R0b*iba)
    Psc = Pcp + Ppmp
    Pdb = Pd + Psc - Ppv - Pfc - Pmt
    Iba_cal = Pdb*1000/Vba
    
    dxdt[0] = epsilon_1*(zfc*((Ifc - If)/tau_e))
    dxdt[1] = epsilon_1*(zfc*((Gff_mol - Gh2)/tau_f))
    dxdt[2] = epsilon_1*(zfc*((1/KO2*(Go2 - If*Kr) - pO2)/tau_O2))
    dxdt[3] = epsilon_1*(zec*((Gr*(hcri - hcro) + alfar*Aci*(tcs - tc))/(Ccr*Mcr)))
    dxdt[4] = epsilon_1*(zec*((alfar*Aci*(tc - tcs) + alfaw*Aco*(tcwm - tcs))/(Cs*Mcs)))
    dxdt[5] = epsilon_1*(zec*((Gr*(heri - hero) + alfar*Aei*(tes - te))/(Cer*Mer)))
    dxdt[6] = epsilon_1*(zec*((alfar*Aei*(te - tes) + alfaw*Aeo*(tewm - tes))/(Cs*Mes)))
    dxdt[7] = epsilon_1*(iba/C1b - vcap/(R1b*C1b))
    dxdt[8] = epsilon_1*((Iba_cal - Iba)/tau_dc)
    
    return dxdt

# Output
def out_ies_f(xf, xs, xm, us, um, uf, uz, ud):
    y_alg = MX.zeros(Ny_f)
    
    SOC = xs[0]
    mstc = xs[1]
    mt_stc = xs[2]
    mt_sth = xs[3]
    tbr = xs[4]
    
    pH2O = xm[0]
    pH2 = xm[1]
    Pmtf = xm[2]
    tabf = xm[3]
    tabw = xm[4]
    tabt = xm[5]
    tcwm = xm[6]
    tewm = xm[7]
    tre = xm[8]
    
    If = xf[0]
    Gh2 = xf[1]
    pO2 = xf[2]
    tc = xf[3]
    tcs = xf[4]
    te = xf[5]
    tes = xf[6]
    vcap = xf[7]
    Iba = xf[8]
    
    Gab = us[0]
    Gec = us[1]
    Gstu = us[2]
    
    Gfm = um[0]
    
    Gff = uf[0]
    Nec = uf[1]
    
    zfc = uz[0]
    zma = uz[1]
    zec = uz[2]
    zst = uz[3]
    
    ta = ud[0]
    ins = ud[1]
    Pd = ud[2]
    Qother = ud[3]
        
    # PV
    I_pv_max = I_max_0*ins/Ins_pv_0*(1 + c_pv1*(ta - T_pv_0))
    V_pv_max = V_max_0*log(exp(1) + c_pv2*(ins - Ins_pv_0))*(1 - c_pv3*(ta - T_pv_0))
    Ppv = npp*nsp*I_pv_max*V_pv_max/1000
        
    # FC
    eta_a = afc + bfc*log(If + eps)
    eta_c = -R0fc*T0fc/(2*F0)*log(1-If/IL + eps)
    eta_o = If*rfc
    V0 = N0fc*(E0fc + R0fc*T0fc/(2*F0)*log(pH2*sqrt(pO2)/(pH2O + eps) + eps))
    Vfc = V0 - eta_a - eta_c - eta_o
    Ifc = fu_d/(2*Kr)*Gh2
    Pfc = zfc*(Vfc*Ifc/1000)
    
    # PIP in the rerurn side
    Gall = Gab + Gec + Gstu
        
    # MA
    Pmt = zma*(Pmt0 + Pmtf)
    
    # EC
    pc = exp(21.3-2025.5/(248.94+tc))/1e6
    pe = exp(21.3-2025.5/(248.94+te))/1e6
    yb = pc/pe # Compressor
    etavl = 0.98-0.085*(yb**(1/kr)-1)
    etacp = 0.9085*exp(-0.06443*yb)-7.605*exp(-3.155*yb)
    Gr = etavl*Nec*roeg*Vcp
    wi = kr/(1e3*(kr-1))*pe*1e6/roeg*(yb**((kr-1)/kr)-1)
    Pcp = zec*(Gr*wi/etacp)
    
    # BR
    Hpmp = Spmp*Gall**2/(rhow*ge)
    eta_pmp = b0p*Gall**2 + b1p*Gall + b2p
    Ppmp = Gall*ge*Hpmp/(1e3*eta_pmp)
    
    # BA
    iba = Iba/npb
    Vba = nsb*(Em - vcap - R0b*iba)
    Pba = Vba*Iba/1000
    
    # Electric Power
    Psc = Pcp + Ppmp
    Psl = Ppv + Pfc + Pmt + Pba - Psc
    
    y_alg[0] = Psl
    y_alg[1] = tbr
        
    return y_alg

# In[2] Formulate discrete time dynamics {xf, xs, xm, us, um, uf, uz, ud}
x_sym_f = SX.sym('x_f', Nx_f)
x_s_sym_f = SX.sym('x_s_f', Nx_s)
x_m_sym_f = SX.sym('x_m_f', Nx_m)

u_s_sym_f = SX.sym('u_s_f', Nuc_s)
u_m_sym_f = SX.sym('u_m_f', Nuc_m)
u_sym_f = SX.sym('u_f', Nuc_f)

uz_sym_f = SX.sym('uz_f', Nuz_l)
ud_sym_f = SX.sym('ud_f', Nud_f)

ode_sym_f = ode_ies_f(x_sym_f, x_s_sym_f, x_m_sym_f, u_s_sym_f, u_m_sym_f, u_sym_f, 
                      uz_sym_f, ud_sym_f)
args_f = {'x': x_sym_f, 'p': vertcat(x_s_sym_f, x_m_sym_f, u_s_sym_f, u_m_sym_f, u_sym_f, 
                                     uz_sym_f, ud_sym_f), 'ode': ode_sym_f}
opts_f = {'tf': Delta_f, 'regularity_check': True}
I_ode_f = integrator('I_ode_f', 'rk', args_f, opts_f)
